// Feature_MFCC.cpp: implementation of the CFeature_MFCC class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "LXH_Feature_MFCC.h"
#include "stdio.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CFeature_MFCC::CFeature_MFCC()
{
	para.MFCC_Order = 12;			// number of MFCC
	para.NFilter = 20;			// number of filters in Filter bank 
	para.DEnergy = 1;			// 1: output delta value for first MFCC (energy)
	para.use_power_rather_than_energy = false;
	para.take_log = true;
	para.MFCC_Lift = 22;			//Coeff of liftering : default 22
	para.include_co = false;			//default true;
}

CFeature_MFCC::~CFeature_MFCC()
{

}

/*
Output :
	CData::fMat : MFCC
	CData::pNext->fMat : Filter bank
*/
CData* CFeature_MFCC::ExtractFeature()
{
	if( fSignal == NULL )
		ErrorMsg("You must load signal before : CopySignal");

	float *s = new float [para.WinSz];

	int NumWin = (nLen - para.WinSz) / para.StpSz + 1;
	CData *pMFCC = new CData(NumWin, para.MFCC_Order);
	//CData *pFBank = new CData(NumWin, para.NFilter );
	CData *fbank, *mfcc;
	
	for (int i = 0; i<NumWin; i++)
	{
		memcpy( s, fSignal + i*para.StpSz, para.WinSz * sizeof(float) );
		//if(para.PreEmphasis) 
		//	PreEmphasis( s,para.WinSz, float(para.Alpha));
		if(para.Windowing) 
			Window( s, para.WinSz );
		fbank = Signal2FBank( s, para.WinSz, para.SRate, para.NFilter, true, true );
		mfcc = FBank2MelCep( fbank->fMat[0], fbank->nCol, para.MFCC_Order, float(para.MFCC_Lift), para.include_co );
		memcpy( pMFCC->fMat[i], mfcc->fMat[0], para.MFCC_Order * sizeof(float));
	//	memcpy( pFBank->fMat[i], fbank->fMat[0], para.NFilter * sizeof(float));
		delete fbank;
		delete mfcc;
	}

	//pMFCC->Append( *pFBank );
	//delete pFBank;
	delete[] s;
	return pMFCC;

	/*FBankInfo	fbinfo;
	FloatVec	fbank;
	FloatVec	mfcc;
	FloatVec	s;
	float		te;

	if( fSignal == NULL )
		ErrorMsg("You must load signal before : CopySignal");
	
	fbinfo = InitFBank( para.WinSz, ((DWORD)10000000/para.SRate), para.NFilter, LOPASS, HIPASS, TRUE, TRUE);
	fbank = CreateFloatVec(para.NFilter);
	mfcc = CreateFloatVec(para.MFCC_Order);


	int NumWin = (nLen - para.WinSz) / para.StpSz + 1;

	CData *pMFCC = new CData(NumWin, para.MFCC_Order);
	
	s = CreateFloatVec(para.WinSz);

	for (int i = 0; i<NumWin; i++)
	{
		memcpy( s + 1, fSignal + i*para.StpSz, para.WinSz * sizeof(float) );
		PreEmphasise( s, float(para.Alpha));
		Hamming(s);
		Wave2FBank(s, fbank, &te, fbinfo);
		FBank2MFCC(fbank, mfcc, para.MFCC_Order);
		// Nhan trong so 
		int m_nNumberCepLifter = para.MFCC_Order;
		//WeightCepstrum(mfcc, 1, para.MFCC_Order, m_nNumberCepLifter );
		for (int j = 1; j <= para.MFCC_Order; j++) (pMFCC->fMat)[i][j-1] = mfcc[j];
	}

	free(s);
	free(fbank);
	free(mfcc);

	free(fbinfo.cf);
	free(fbinfo.loWt);
	free(fbinfo.x);
	free(fbinfo.loChan);
	return pMFCC;
	*/
}

void CFeature_MFCC::Weight(CData &data)
{
	if( data.fMat==NULL ) return;
	int i,j;
	CData *p;

	float	a = float(PI/data.nCol);
	float	Lby2 = data.nCol/2.0f;

	for( i = 0; i<data.nRow; i++ )
	{
		for( j=0; j<data.nCol; j++ )
			(data.fMat)[i][j] *= float(1.0 + Lby2 * sin((j+1) * a)); 
	}

	p = data.pNext;
	while( p != NULL )
	{
		for( i = 0; i<p->nRow; i++ )
		{
			for( j=0; j<p->nCol; j++ )
				(p->fMat)[i][j] *= float(1.0 + Lby2 * sin((j+1) * a)); 
		}
		p = p->pNext;
	}
}

void CFeature_MFCC::FFT2FBank(float *fft_frame, int fft_len, float *fbank_vec, int fbank_len, float Hz_per_fft_coeff, const float *mel_fbank_frequencies, int mbank_len)
{
	// expects "half length" FFT - i.e. energy or power spectrum
    // energy is magnitude; power is squared magnitude

    // mel_fbank_frequencies is a vector of centre frequencies
    // BUT : first element is lower bound of first filter
    //       last element is upper bound of final filter
    // i.e. length = num filters + 2

    int		i,k;
    float	this_mel_centre,this_mel_low,this_mel_high;
    float	*filter = NULL;
	int		fil_len;
    int		fft_index_start;


    // check that fbank_vec and mel_fbank_frequencies lengths match
    if(mbank_len != fbank_len + 2)
    {
		char errmsg[256];
		sprintf(errmsg,"Filter centre frequencies length (%d) is not equal to fbank order (%d) plus 2\n",
			mbank_len, fbank_len);
		ErrorMsg(errmsg);
    }

    // filters are computed on the fly
    for(i=0;i<fbank_len;i++)
    {

		// work out shape of the i'th filter
		this_mel_low = mel_fbank_frequencies[i];
		this_mel_centre=mel_fbank_frequencies[i+1];
		this_mel_high=mel_fbank_frequencies[i+2];
	
		filter = make_mel_triangular_filter(this_mel_centre,this_mel_low,this_mel_high, Hz_per_fft_coeff, fft_len, fft_index_start, fil_len);

 		// do filtering
		fbank_vec[i]=0.0;
		for(k=0; k<fil_len;k++)
			fbank_vec[i] += fft_frame[fft_index_start + k] * filter[k];
		free(filter);
	}
}

/// Round to nearest integer
static inline int irint(float f) { return (int)(f+0.5); }
/// Round to nearest integer
static inline int irint(double f) { return (int)(f+0.5); }
/*
Output
	filter	: Pointer to filter, must be delete after call this function
	fil_len : Length of filter
*/
float* CFeature_MFCC::make_mel_triangular_filter(const float this_mel_centre, const float this_mel_low, const float this_mel_high, const float Hz_per_fft_coeff, const int half_fft_order, int &fft_index_start,  int &fil_len)
{
	// makes a triangular (on a Mel scale) filter and creates
    // a weight vector to apply to FFT coefficients

    int i,filter_vector_length,fft_index_stop;
    float rise_slope,fall_slope,this_mel;

    // slopes are in units per Mel
    // this is important - slope is linear in MEl domain, not Hz
    rise_slope = 1/(this_mel_centre - this_mel_low);
    fall_slope = 1/(this_mel_centre - this_mel_high);


    // care with rounding - we want FFT indices **guaranteed**
    // to be within filter so we get no negative filter gains
    // (irint gives the _nearest_ integer)

    // round up
    if(this_mel_low == 0)
	fft_index_start=0;
    else
		fft_index_start = irint(0.5f + (Mel2Hz(this_mel_low) / Hz_per_fft_coeff));

    // round down
    fft_index_stop = irint((Mel2Hz(this_mel_high) / Hz_per_fft_coeff) - 0.5);

    if(fft_index_stop > half_fft_order-1)
	fft_index_stop = half_fft_order-1;


    filter_vector_length = fft_index_stop - fft_index_start + 1;
	fil_len = filter_vector_length;
    float *filter = (float*) calloc(fil_len,sizeof(float));

	//Add by LE Xuan Hung
	if( (filter_vector_length==1) )
	{
		filter[0] = 1;
		return filter;
	}
	//Finish

    for(i=0;i<filter_vector_length;i++)
    {
		this_mel = Hz2Mel( (i + fft_index_start) * Hz_per_fft_coeff );
	
		if(this_mel <= this_mel_centre)
		{
			filter[i] = rise_slope * (this_mel - this_mel_low);
		}
		else
		{
			filter[i] = 1 + fall_slope * (this_mel - this_mel_centre);
		}
    }
	return filter;
}

float CFeature_MFCC::Mel2Hz(float frequency_in_Mel)
{
	return float((exp(frequency_in_Mel / 1127) - 1) * 700);
}


float CFeature_MFCC::Hz2Mel(float frequency_in_Hertz)
{
	return float(1127 * log(1 + frequency_in_Hertz/700.0));
}

CData * CFeature_MFCC::Signal2FBank(int nNumberOfBanks)
{
	//ASSERT( nNumberOfBanks > 0 );
	if( nNumberOfBanks < 0 ) 
		ErrorMsg("Eorr in Signal2FBank, Number of banks must > 0");
	if( fSignal == NULL )
		ErrorMsg("You must load signal before : CopySignal");

	float *s = new float [para.WinSz];

	int NumWin = (nLen - para.WinSz) / para.StpSz + 1;
	CData *pBank = new CData(NumWin, nNumberOfBanks);
	//CData *pFBank = new CData(NumWin, para.NFilter );
	CData *fbank;
	
	for (int i = 0; i<NumWin; i++)
	{
		memcpy( s, fSignal + i*para.StpSz, para.WinSz * sizeof(float) );
		//if(para.PreEmphasis) 
		//	PreEmphasis( s,para.WinSz, float(para.Alpha));
		if(para.Windowing) 
			Window( s, para.WinSz );
		fbank = Signal2FBank( s, para.WinSz, para.SRate, nNumberOfBanks, true, true );
		memcpy( pBank->fMat[i], fbank->fMat[0], nNumberOfBanks * sizeof(float));
		delete fbank;
	}

	//pMFCC->Append( *pFBank );
	//delete pFBank;
	delete[] s;
	return pBank;
}

#define SAFE_LOG_ZERO -9538

inline double safe_log(const double x)
{
    double l;
    if (x == 0)
	return SAFE_LOG_ZERO;
    l=log(x);
    if (l<SAFE_LOG_ZERO)
	return SAFE_LOG_ZERO;
    else
	return l;
}

CData * CFeature_MFCC::Signal2FBank(const float *sig, int sig_len, const int sample_rate, const int fbank_len, const bool use_power_rather_than_energy, const bool take_log)
{
	int		fft_len;
    int		i,fbank_order;
    float	Hz_per_fft_coeff;

	CData *fbank = new CData(1, fbank_len);

    // upper and lower limits of filter bank
    // where the upper limit depends on the sampling frequency
    // TO DO : add low/high pass filtering HERE
    float mel_low = 0;
    float mel_high = Hz2Mel( sample_rate /2.0f);

    // FFT this frame. FFT order will be computed by sig2fft
    // FFT frame returned will be half length of actual FFT performed
	CData *pFFT = Signal2FFT( sig, sig_len, use_power_rather_than_energy);
	fft_len = pFFT->nCol;

    // this is more easily understood as half the sampling
    // frequency over half the fft order, but fft_frame_length()
    // is already halved
    Hz_per_fft_coeff = 0.5f * sample_rate / fft_len;

    fbank_order = fbank_len;

    // store the list of centre frequencies and lower and upper bounds of
    // the triangular filters
	int mbank_len = fbank_order+2;
    float *mel_fbank_centre_frequencies = (float*)calloc( mbank_len, sizeof(float));
    
    mel_fbank_centre_frequencies[0]=mel_low;

    for(i=1;i<=fbank_order;i++)
	mel_fbank_centre_frequencies[i] = mel_low + 
	    (float)(i) * (mel_high - mel_low) / (fbank_order+1);

    mel_fbank_centre_frequencies[fbank_order+1]=mel_high;

    // bin FFT in Mel filters
    FFT2FBank( pFFT->fMat[0], fft_len, fbank->fMat[0], fbank->nCol, Hz_per_fft_coeff, mel_fbank_centre_frequencies, mbank_len);
    
    if(take_log)
	for( i=0;i<fbank->nCol;i++)
	    fbank->fMat[0][i] = float(safe_log(fbank->fMat[0][i]));

	delete pFFT;
	free(mel_fbank_centre_frequencies);
	return fbank;
}

CData * CFeature_MFCC::FBank2MelCep(CData &fbank)
{
	if( fbank.fMat == NULL ) return NULL;
	
	CData *pMFCC = FBank2MelCep( fbank.fMat[0], fbank.nCol, para.MFCC_Order, float(para.MFCC_Lift), para.include_co );
	for( int i = 1; i < fbank.nRow; i++ )
	{	
		CData *pTemp;
		pTemp = FBank2MelCep( fbank.fMat[i], fbank.nCol, para.MFCC_Order, float(para.MFCC_Lift), para.include_co );
		pMFCC->Append( *pTemp );
		delete pTemp;
	}

	CData *pCur = fbank.pNext;
	while( pCur != NULL )
	{
		for( i = 0; i < pCur->nRow; i++ )
		{	
			CData *pTemp;
			pTemp = FBank2MelCep( pCur->fMat[i], pCur->nCol, para.MFCC_Order, float(para.MFCC_Lift), para.include_co);
			pMFCC->Append( *pTemp );
			delete pTemp;
		}
		pCur = pCur->pNext;
	}

	return pMFCC;
}

CData * CFeature_MFCC::FBank2MelCep(const float *fbank_vec,const int fbank_len, const int nMFCCNumber, const float liftering_parameter, const bool include_c0)
{
	// a cosine transform of the fbank output
    // remember to pass LOG fbank params (energy or power)
	CData *pMFCC = new CData(1,nMFCCNumber);
    int i,j,actual_mfcc_index;
    float pi_i_over_N,const_factor;
    float PI_over_liftering_parameter;

    if(liftering_parameter != 0.0)
		PI_over_liftering_parameter = float(PI / liftering_parameter);
    else
	PI_over_liftering_parameter = (float)PI; // since sin(n.PI) == 0

    const_factor = float( sqrt( 2.0f/fbank_len));

    for(i=0;i<nMFCCNumber;i++)
    {
		actual_mfcc_index = include_c0 ? i : i+1;
		pi_i_over_N  = float(  PI * (float)(actual_mfcc_index) / (float)(fbank_len));
		float *mfcc_vec = pMFCC->fMat[0];
		for(j=0;j<fbank_len;j++)
			// j + 0.5 is because we want (j+1) - 0.5
		    mfcc_vec[i] += float(fbank_vec[j] * cos(pi_i_over_N * ((float)j + 0.5)));	
		mfcc_vec[i] *= const_factor;
		// liftering
		mfcc_vec[i] *= float(1 + (0.5 * liftering_parameter  * sin(PI_over_liftering_parameter * (float)(actual_mfcc_index))));
    }
	return pMFCC;
}

float CFeature_MFCC::MFCCdistance(CData Unit1, CData Unit2)
{
	if((Unit1.nCol !=Unit2.nCol)||(Unit1.nRow!=Unit2.nRow)) return -1;

	float fSum=0;
	//float fUnit1E =0, fUnit2E =0;
	int i,j;
	
/*	for(i = 0; i <Unit1.nRow; i++)
		for(j = 0; j<Unit1.nCol; j++)
		{
			fUnit1E = fUnit1E + Unit1.fMat[i][j]*Unit1.fMat[i][j];
			fUnit2E = fUnit2E + Unit2.fMat[i][j]*Unit2.fMat[i][j];
		}
	fUnit1E = sqrt(fUnit1E);
	fUnit2E = sqrt(fUnit2E);

	for(i = 0; i <Unit1.nRow; i++)
		for(j = 0; j<Unit1.nCol; j++)
		{
			fSum = fSum + (Unit1.fMat[i][j]/fUnit1E - Unit2.fMat[i][j]/fUnit2E)*(Unit1.fMat[i][j]/fUnit1E - Unit2.fMat[i][j]/fUnit2E);
		}
		fSum = sqrt(fSum);*/
	//return(fSum);

	
	float sum_sq_x = 0;
	float sum_sq_y = 0;
	float sum_coproduct = 0;
	float sweep, delta_x,delta_y;
	float mean_x = Unit1.fMat[0][0];
	float mean_y = Unit2.fMat[0][0];
	int nCounter =2;
	for(i = 0; i <Unit1.nRow; i++)
		for(j = 0; j<Unit1.nCol; j++)
		{
			if((i==0)&&(j==0)) continue;
			sweep = float(nCounter - 1.0) / nCounter;
			delta_x = Unit1.fMat[i][j] - mean_x;
			delta_y = Unit2.fMat[i][j] - mean_y;
			sum_sq_x += delta_x * delta_x * sweep;
			sum_sq_y += delta_y * delta_y * sweep;
			sum_coproduct += delta_x * delta_y * sweep;
			mean_x += delta_x / nCounter;
			mean_y += delta_y / nCounter;
			nCounter++;
			
		}
	float pop_sd_x = sqrt( sum_sq_x / nCounter );
	float pop_sd_y = sqrt( sum_sq_y / nCounter );
	float cov_x_y = sum_coproduct / nCounter;
	float correlation = cov_x_y / (pop_sd_x * pop_sd_y);
	
	//return correlation;
	return float(2*(1.0-correlation));
	
}
